#define F_CPU 20000000UL  // 20 MHz CPU clock
#define	STEPSIZE_ticks 75.625

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "serial.h"

// Global variables
Serialt serial;
uint8_t charIn; 								//USART incoming character
char str[12];
int isEnabled = 1; 
double pwm_ticks = 1270;
int timercount = 0;
int nSeconds = 0;
int isSequence = 0;
int holenumber = 0;
int tick = 0;

void set_pwm(int pwm)
{
	OCR1BH = pwm >> 8;
	OCR1BL = pwm & 0xFF;

	return;
}

void stepTo(int holenumber)
{
	switch(holenumber)
	{
		case 0:
			set_pwm( 1270 );
			break;
		case 1:
			set_pwm( 1340 );
			break;
		case 2:
			set_pwm( 1422 );
			break;
		case 3:
			set_pwm( 1498 );
			break;
		case 4:
			set_pwm( 1574 );
			break;
		case 5:
			set_pwm( 1650 );
			break;
		case 6:
			set_pwm( 1726 );
			break;
		case 7:
			set_pwm( 1802 );
			break;
		case 8:
			set_pwm( 1878 );
			break;
		case 9:
			set_pwm( 1828 );
			break;
		case 10:
			set_pwm( 1904 );
			break;
		case 11:
			set_pwm( 2102 );
			break;
		case 12:
			set_pwm( 2178 );
			break;
		case 13:
			set_pwm( 2253 );
			break;
		case 14:
			set_pwm( 2329 );
			break;
		case 15:
			set_pwm( 2404 );
			break;
		case 16:
			set_pwm( 2480 );
			break;
		case 17:
			set_pwm( 2556 );
			break;
		default:
			// ignore/throw error?
			break;
	}
}
void forward_step( void )
{
	pwm_ticks = pwm_ticks + STEPSIZE_ticks; 	//	Leftmost position
	set_pwm( (int)pwm_ticks );
}

void back_step( void )
{
	pwm_ticks = pwm_ticks - STEPSIZE_ticks; 	//	Leftmost position
	set_pwm( (int)pwm_ticks );
}

void reset( void )
{
	isSequence = 0;
	holenumber=0;
	stepTo(holenumber);
}

ISR (TIMER1_COMPB_vect) // Timer 1 is a 16-bit timer
{
	//This timer is for the pwm
	tick=tick+1;
}

void commMonitor(void)
{
	// Deal with incoming serial bytes here
	if (serial_read(&serial,&charIn)) //if there's data read it
	{
		serial_write(&serial, charIn); //echo it back out for now...
		switch (charIn)
		{
			case 'a':
			//	"Abort" stops the sequence
				isSequence = 0;
				sprintf(str,"\rAborted... ");
				for(int i=0; i<12; i++)
					serial_write(&serial, str[i]);
				break;
			case 'r':
				//"Reset" go to leftmost position
				sprintf(str,"\rResetting. ");
				for(int i=0; i<12; i++)
					serial_write(&serial, str[i]);
				reset();
				break;
			case 'g':
			//	'Go' do sequence
				sprintf(str,"\rStarting.. ");
				for(int i=0; i<12; i++)
					serial_write(&serial, str[i]);
				isSequence = 1;
				timercount = 80;
				nSeconds = 0;
				holenumber=0;
				break;
			case '[':
			//	Go to leftmost position
				pwm_ticks = 1270; 	//	Leftmost position
				set_pwm( (int)pwm_ticks );
				break;
			case ']':
			//	Go to full right position
				pwm_ticks = 2480;	//	Rightmost position
				set_pwm( (int)pwm_ticks );
				break;
			case 'f':
			//	"Forward" forward one step
				forward_step();
				break;
			case 'b':
			//	"Back" back one step
				back_step();			
				break;

			default:
				//	Throw error?
				break;
		}

	}

	return;
}


int main(void) 
{
	
	// Initialization
	
	// 	Set up serial port
	serial_init(&serial);
	
	//	Set up IO pin
	DDRC |= _BV(PC1); 	// set led pin to output direction
	PORTC |= _BV(PC1); 	// led on, pin high
	DDRB |= _BV(PB2);	// enable pwm output pin
	
	//	Set up pwm timer
	TCCR1A |= _BV(5); //WGM's cleared for PWM phase and freq correct mode, non-inverting output, 
								//Compare Match B
	TCCR1B |= _BV(4) | _BV(1); //Clock prescaler 8
	ICR1H = 0x61; //0x61A8 is 25000 as TOP for 50 Hz
	ICR1L = 0xA8;
	TIMSK1 |= _BV(2); // Enable timer interrupt OCIE1B

	//	Set up the 10s timer
	/*TCCR0A |= _BV(7) | _BV(6) | _BV(1); 
	TCCR0B |= _BV(2) | _BV(0); // prescaler 1024
	OCR0A = 0xF1; // Compare match at this value
	TIMSK0 |= _BV(1); // Enable timer interrupt
	*/

	//	Set pwm value here
	set_pwm( (int)1270 ); 

	// Start timers
	TCNT1H = 0; // Set timer high bits
	TCNT1L = 0; // Set timer low bits
	TCNT0 = 0;

	sei(); // Globally enable interrupts
         
    while (1)                       /* loop forever */
    {
		commMonitor();	// Monitor serial port for incoming bytes
		if(isSequence)
		{
			//	Interrupt triggers at 81 Hz;
			if(tick >= 100)
			{
				sprintf(str,"\r%2.0d...%2.0ds   ", holenumber,nSeconds);
				for(int i=0; i<12; i++)
					serial_write(&serial, str[i]);
				nSeconds--;
				tick = 0;
			}
			if(nSeconds<0)
			{
				nSeconds = 1;
				stepTo(holenumber++);
				if(holenumber > 18)
					reset();
			}
		}
    }
	
	
	return 0;
}
